/* LICENSE: */

#ifndef __ROBOT_TRACKER_H__
#define __ROBOT_TRACKER_H__

#include <reality/net_vision.h>
#include "kalman.h"

class RobotTracker : private Kalman {
private:
  int type;
  bool use_commands;
  double latency;

  int reset_on_obs;

  struct rcommand {
    double timestamp;
    vector3d vs;
  };

  std::deque<rcommand> cs; // Velocity commands

  rcommand get_command(double time);

protected:
  virtual Matrix& f(const Matrix &x, Matrix &I); // noiseless dynamics
  virtual Matrix& h(const Matrix &x); // noiseless observation

  virtual Matrix& Q(const Matrix &x); // Covariance of propagation noise
  virtual Matrix& R(const Matrix &x); // Covariance of observation noise

  virtual Matrix& A(const Matrix &x); // Jacobian of f w.r.t. x
  virtual Matrix& W(const Matrix &x); // Jacobian of f w.r.t. noise
  virtual Matrix& H(const Matrix &x); // Jacobian of h w.r.t. x
  virtual Matrix& V(const Matrix &x); // Jacobian of h w.r.t. noise

public:
  RobotTracker(int _type = ROBOT_TYPE_NONE,
	       bool _use_commands = true,
	       double _latency = LATENCY_DELAY);
  virtual ~RobotTracker() {}

  void set_type(int _type, bool _use_commands = true) { 
    type = _type; use_commands = _use_commands; reset(); }

  void reset() { reset_on_obs = true; }
  void reset(double timestamp, float state[6]);

  void observe(vraw obs, double timestamp);
  void command(double timestamp, vector3d vs);

  vector2d position(double time);
  vector2d velocity(double time);
  vector2d velocity_raw(double time);
  double direction(double time);
  double angular_velocity(double time);

  double stuck(double time);
};

#endif
